﻿#include "JPDADataAssociation.h"
#include "timer_util.h"
#include "lap.h"
#include "MTTUtils.h"



#ifndef M_PI 
#define M_PI 3.141592653
#endif

//JPDA算法中的杂波偏置参数
#define JPDA_B_PARAM	(0/ 100.0)
//对于机动目标其过程噪声放大倍数
#define ManeuverTargetQZoom	(30)
//处于静止状态的目标由于雷达误差所引起的漂移的最大距离上限
#define STOP_PLOT_DRIFT_LIMIT	(150.0)
typedef struct conflictTrackInfo {
	double score;
	size_t trackid;
	bool operator<(const conflictTrackInfo& rv) {
		return score < rv.score;
	}
}conflictTrackInfo_t;

/*
void JPDADataAssociation::detectTargetManeuver(TrackInfo& track) {
	bool is_maneuver = track.is_maneuvering();
	if (is_maneuver) {
		//如果目标正在机动，则增加过程噪声
		track.UKF.Q = track.Q_ORI * ManeuverTargetQZoom;
	}
	else {
		//恢复正常的过程噪声
		track.UKF.Q = track.Q_ORI;
	}
}
*/
//-----------------------------------------

JPDADataAssociation::JPDADataAssociation(onDeleteTrackAction delAct)
	:globalID(1),
	deletetrackAct(delAct),
	B(JPDA_B_PARAM),
	current_timeSecFromEpoch(0),
	algo_type(NNJPDA),
	max_trackVel_ms(20) {
}
JPDADataAssociation::~JPDADataAssociation() {

}

void JPDADataAssociation::addNewTentativeTrack(TentativeTrack& trace) {
	unsigned int NID = getNewID();
	//TrackInfo track(NID, trace);
	//tracks.push_back(track);
	tracks.emplace_back(NID, trace);
}

double JPDADataAssociation::likelihood_tj(size_t t, size_t j,
	const std::vector<DistributionParam_t>& pred_z_dist,
	const CCVBlockVec_t& plots) {

	if (likelihoodDelay(t, j) == 0) {
		//之前没有计算航迹t与j的关联概率，延迟到现在进行计算

		VectorXd x(2);
		x << plots.at(j).Range, plots.at(j).Azimuth;
		//double likelihood = NormalDisPdf(2, x, pred_z_dist[t].mean, pred_z_dist[t].P, RD_2D_residual_func);
		double likelihood = NormalDisPdf_Ex(2, x,
			pred_z_dist[t].mean,
			pred_z_dist[t].P,
			pred_z_dist[t].P_det,
			pred_z_dist[t].P_Inverse,
			RD_2D_residual_func);

		likelihoodDelay(t, j) = likelihood; //保存计算结果
		return likelihood;
	}
	else {
		return likelihoodDelay(t, j);
	}


	//VectorXd x(2);
	//x << plots.at(j).Range, plots.at(j).Azimuth;
	////double likelihood = NormalDisPdf(2, x, pred_z_dist[t].mean, pred_z_dist[t].P, RD_2D_residual_func);
	//double likelihood = NormalDisPdf_Ex(2, x, 
	//	pred_z_dist[t].mean, 
	//	pred_z_dist[t].P, 
	//	pred_z_dist[t].P_det ,
	//	pred_z_dist[t].P_Inverse,
	//	RD_2D_residual_func);
	//return likelihood;
}
double JPDADataAssociation::N_tj(size_t t, size_t j,
	const std::vector<DistributionParam_t>& pred_z_dist,
	const CCVBlockVec_t& plots,
	std::vector<std::set<size_t> >& A) {
	if (NtjDelay(t, j) == 0) {
		double ntj = N_tj_(t, j, pred_z_dist, plots, A);
		NtjDelay(t, j) = ntj;
		return ntj;
	}
	else {
		return NtjDelay(t, j);
	}
}

//计算Ntj
double JPDADataAssociation::N_tj_(size_t t, size_t j,
	const std::vector<DistributionParam_t>& pred_z_dist,
	const CCVBlockVec_t& plots,
	std::vector<std::set<size_t> >& A) {

	/*
	如果航迹t的门内不包含任何航迹，概率=0
	*/
	size_t At_size = A[t].size();
	if (At_size == 0)
		return 0;

	if (At_size == 1) {
		/*航迹t的门内只有一个点迹,计算航迹t与量测j的互联概率*/
		double likelihood = likelihood_tj(t, j, pred_z_dist, plots);
		return likelihood;
	}
	else {
		/*航迹t的门内包含多个量测,取除了j之外的最大概率*/
		double max_likelihood = -1;
		auto it = A[t].begin();
		auto it_end = A[t].end();
		for (; it != it_end; ++it) {
			size_t k = *it;
			if (k == j)
				continue;
			double likelihood = likelihood_tj(t, k, pred_z_dist, plots);
			if (likelihood > max_likelihood)
				max_likelihood = likelihood;
		}
		return max_likelihood;
	}
}

double JPDADataAssociation::D_tj(size_t t, size_t j,
	const std::vector<DistributionParam_t>& pred_z_dist,
	const CCVBlockVec_t& plots,
	std::vector<std::set<size_t> >& A,
	std::vector<std::set<size_t> >& L) {

	if (L[t].size() == 0)
		return likelihood_tj(t, j, pred_z_dist, plots);
	else {
		double likelihood = likelihood_tj(t, j, pred_z_dist, plots);
		double likelihood_sum = 0;
		for (auto it = L[t].begin(); it != L[t].end(); ++it) {
			size_t u = *it;
			likelihood_sum += N_tj(u, j, pred_z_dist, plots, A);
		}
		return likelihood * (likelihood_sum);
	}
}

static double CalcIncludedAngle_(
	const TrackPoint2D_t& plot_0, //第一个点
	const TrackPoint2D_t& plot_1, //第二个
	const TrackPoint2D_t& a_plot //第三个点点
) {
	double a = POW2(a_plot.X - plot_1.X) + POW2(a_plot.Y - plot_1.Y);
	double b = POW2(plot_1.X - plot_0.X) + POW2(plot_1.Y - plot_0.Y);
	double c = POW2(a_plot.X - plot_0.X) + POW2(a_plot.Y - plot_0.Y);
	//使用三角形的点乘公式计算新点迹与之前航迹的夹角
	double alpha = acos((a + b - c) / (2 * sqrt(a * b)));
	alpha = 180 - alpha * 180 / M_PI;
	return alpha;
}
/*
论文参考:Suboptimal JPDA for tracking in the presence of clutter and missed detections
*/
void JPDADataAssociation::associatePlot(const CCVBlockVec_t& plots, //当前周期的量测点集
	double  secsPreCycle, //当前的扫描周期（秒）
	int64_t current_timeSecFromEpoch,//当前周期的精确时间戳（精确到毫秒）
	std::vector<size_t>& assignedStatus) {

	stop_timer f_timer;

	//保存原始的点迹排除列表
	std::vector<size_t> assignedStatusOrigin = assignedStatus;

	this->current_timeSecFromEpoch = current_timeSecFromEpoch;
	//assignedStatus.resize(plots.size());

	//保存航迹t的预测参数(均值和方差)
	std::vector<DistributionParam_t> pred_z_dist;
	pred_z_dist.resize(tracks.size());

	/*For each track t, form At, which is the list of indices
	of the validated measurements for track t.*/
	std::vector<std::set<size_t> > A;
	A.resize(tracks.size());

	/*
	For each measurement j, form Cj, which is the list
	of indices of the tracks which validate measurement j.
	*/
	std::vector<std::set<size_t> > C;
	C.resize(plots.size());

	/*For each track t, form the union of all track index
	lists from all the measurements that are validated
	by track t, while excluding the index of track t. */
	std::vector<std::set<size_t> > L;
	L.resize(tracks.size());


	/*
	//计算每个航迹t与哪些量测相关联
	i - 航迹索引
	j - 量测索引
	*/
	size_t trackSize = tracks.size();
	for (size_t t = 0; t < trackSize; t++) {
		TrackInfo& track = tracks.at(t);
		track.history++; //已跟踪帧数增加

		double QScale = 1.0;
		bool is_maneuvering = track.is_maneuvering(&QScale);//目标是否处于机动状态
		//更新滤波器的过程噪声
		double acc_var = POW2(0.02 * QScale);
		MatrixXd QMat = MatrixXd::Zero(4, 4);
		QMat.block<2, 2>(0, 0) = Q_discrete_white_noise(2, secsPreCycle, acc_var);
		QMat.block<2, 2>(2, 2) = Q_discrete_white_noise(2, secsPreCycle, acc_var);
		track.UKF.Q = QMat;

		//目标的预测位置与误差(量测空间)
		DistributionParam_t& pred_z_Pos = track.UKF.predict(secsPreCycle);
		pred_z_Pos.P_det = pred_z_Pos.P.determinant();
		pred_z_Pos.P_Inverse = pred_z_Pos.P.inverse();
		double PredPosR = pred_z_Pos.mean(0, 0); //预测航迹的距离
		double PredPosA = pred_z_Pos.mean(1, 0); //预测航迹的方位
		double X_PrePos = PredPosR * sin(PredPosA / 180.0 * M_PI);
		double Y_PrePos = PredPosR * cos(PredPosA / 180.0 * M_PI);
		pred_z_dist[t] = pred_z_Pos;

		//根据距离和移动状态计算一个最大门限，限制关联计算的目标数量
		double gate_limit = getassociatePlotGate(PredPosR , track.movement_status.status);
	
		size_t plotSize = plots.size();
		for (size_t j = 0; j < plotSize; j++) {
			//如果点迹在排除区域内不进行处理
			if (assignedStatusOrigin[j] > 0)
				continue;

			double dist_offset = sqrt(POW2(X_PrePos - plots.at(j).X) + POW2(Y_PrePos - plots.at(j).Y));
			if (dist_offset > gate_limit)
				continue;

			Vector2d z;
			z << plots.at(j).Range, plots.at(j).Azimuth; //量测
			VectorXd residual_z = RD_2D_residual_func(z, pred_z_Pos.mean);
			//计算量测空间中预测与实际量测的欧式加权距离
			double dk = residual_z.transpose() * pred_z_Pos.P_Inverse * residual_z;
			if (dk < 9.21) {
				//量测在目标的门限内
				A[t].insert(j); //与t航迹相关的所有量测
				C[j].insert(t);//与j量测相关的所有的航迹

				assignedStatus[j] = 1; //标记此Plot已经与某个航迹关联，不在进行航迹的起始操作
			}
		}
	}
	//预先生成Dtj矩阵
	MatrixXd D = MatrixXd::Zero(tracks.size(), plots.size());
	//航迹t的归一化参数
	VectorXd Normal_t = VectorXd::Zero(tracks.size());
	//延迟计算航迹t与j的关联概率
	likelihoodDelay = MatrixXd::Zero(tracks.size(), plots.size());
	//延迟计算Ntj
	NtjDelay = MatrixXd::Zero(tracks.size(), plots.size());

#if 1
	//所有航迹和点迹关联的代价矩阵
	MatrixXd CoastM = MatrixXd::Ones(tracks.size(), plots.size()) * 10;

	//计算代价矩阵
	std::vector<size_t> opt_tracks;
	std::vector<size_t> opt_measures;
	std::set<size_t> opt_measures_set;

	for (size_t t = 0; t < trackSize; t++) {
		TrackInfo& track = tracks.at(t);
		track.AtSize = A[t].size();
		if (A[t].size() == 0) {
			//没有量测关联的目标	
			double x = track.UKF.x(0, 0); //保存历史点迹
			double y = track.UKF.x(2, 0);
			track.track_point_seq.push(TrackPoint2D_t(x, y));
			track.numberOfConsecutiveInvisible++;
			track.timeSecFromEpoch = current_timeSecFromEpoch; //更新时间
		}
		else {
			setUnion(A[t], opt_measures_set, opt_measures_set);
			opt_tracks.push_back(t);

			//1.计算L[t]
			std::set<size_t>& measure_set = A[t];
			for (auto it = measure_set.begin(); it != measure_set.end(); ++it) {
				size_t j = *it;
				setUnion(L[t], C.at(j), L[t]);
			}
			L[t].erase(t); //删除t本身

			//计算与航迹t有关联的航迹中，最高的量测数量
			size_t largestMeasurement = 0;
			for (auto Liter = L[t].begin(); Liter != L[t].end(); Liter++) {
				size_t  u = *Liter;
				if (A[u].size() > largestMeasurement)
					largestMeasurement = A[u].size();
			}

			double Bt = 0;
			if (largestMeasurement >= 2) {
				Bt = 0;
			}
			else {
				//计算Bt
				for (auto Liter = L[t].begin(); Liter != L[t].end(); Liter++) {
					size_t  u = *Liter;
					for (auto At_it = A[t].begin(); At_it != A[t].end(); At_it++) {
						size_t j = *At_it;
						Bt += likelihood_tj(u, j, pred_z_dist, plots);
					}
				}
			}

			//计算航迹t的归一化参数 ,计算与航迹t相关的量测的互联概率
			double normal_D_t = Bt;
			for (auto At_it = A[t].begin(); At_it != A[t].end(); At_it++) {
				size_t j = *At_it;
				double d_tj = D_tj(t, j, pred_z_dist, plots, A, L);
				D(t, j) = d_tj;
				normal_D_t += d_tj;
			}

			for (auto it = A[t].begin(); it != A[t].end(); it++) {
				size_t j = *it;
				double B_tj = D(t, j) / normal_D_t;
				CoastM(t, j) = 1.0 - B_tj;
			} //for
		}
	}

	//优化代价矩阵，精简未关联的航迹与量测
	MatrixXd CoastMOpt = MatrixXd::Ones(opt_tracks.size(), opt_measures_set.size()) * 10;
	int col_idx = 0;
	for (auto it = opt_measures_set.begin(); it != opt_measures_set.end(); it++) {
		size_t mj = *it;
		opt_measures.push_back(mj);
		for (size_t t = 0; t < opt_tracks.size(); t++) {
			size_t tidx = opt_tracks[t];
			CoastMOpt(t, col_idx) = CoastM(tidx, mj);
		}
		col_idx++;
	}

	stop_timer timer; //-------------------开启定时器

	vector<int> assignment;
	double cost = CostMinSolve(CoastMOpt, assignment);//计算代价矩阵的全局最优解，令总代价最小

	for (size_t i = 0; i < opt_tracks.size(); i++) {
		auto t = opt_tracks[i];
		TrackInfo& track = tracks.at(t);
		track.AtSize = A[t].size();

		if (assignment[i] < 0 || (1.0 - CoastM(t, opt_measures[assignment[i]])) < 0.15) {
			//如果某个航迹没有对应的最优量测，或者与最优关联量测的概率过低，则该航迹不进行关联
			double x = track.UKF.x(0, 0); //保存历史点迹
			double y = track.UKF.x(2, 0);
			track.track_point_seq.push(TrackPoint2D_t(x, y));
			track.numberOfConsecutiveInvisible++;
			track.timeSecFromEpoch = current_timeSecFromEpoch; //更新时间
		}
		else {
			int optJ = opt_measures[assignment[i]];  //航迹t的最优关联量测
			//使用最优量测更新
			Vector2d z;
			z << plots[optJ].Range, plots[optJ].Azimuth;
			track.UKF.update(z);		//update
			track.calc_velocity();		//更新速度信息

			double x = track.UKF.x(0, 0); //保存历史点迹
			double y = track.UKF.x(2, 0);
			track.track_point_seq.push(TrackPoint2D_t(x, y));
			track.ClutterDense = plots[optJ].ClutterDense; //更新杂波密度信息
			track.numberOfVisibleFrame++;
			track.numberOfConsecutiveInvisible = 0;
			track.timeSecFromEpoch = current_timeSecFromEpoch; //更新时间			
		}
	}


	timer.stop();
	f_timer.stop();

#else

	MatrixXd CoastM = MatrixXd::Zero(tracks.size(), plots.size());


	//计算代价矩阵
	for (size_t t = 0; t < trackSize; t++) {
		TrackInfo& track = tracks.at(t);
		track.AtSize = A[t].size();

		if (A[t].size() > 0) {
			//1.计算L[t]
			std::set<size_t>& measure_set = A[t];
			for (auto it = measure_set.begin(); it != measure_set.end(); ++it) {
				size_t j = *it;
				setUnion(L[t], C.at(j), L[t]);
			}
			L[t].erase(t); //删除t本身

			//2.计算航迹t的归一化参数
			//计算与航迹t相关的量测的互联概率
			double normal_D_t = B;
			for (auto At_it = A[t].begin(); At_it != A[t].end(); At_it++) {
				size_t j = *At_it;
				double d_tj = D_tj(t, j, pred_z_dist, plots, A, L);
				D(t, j) = d_tj;
				normal_D_t += d_tj;
			}

			for (auto it = A[t].begin(); it != A[t].end(); it++) {
				size_t j = *it;
				double B_tj = D(t, j) / normal_D_t;
				CoastM(t, j) = B_tj;
			} //for
		}
	}

	//计算Lt
	for (size_t t = 0; t < trackSize; t++) {
		TrackInfo& track = tracks.at(t);
		track.AtSize = A[t].size();
		track.history++;

		if (A[t].size() == 0) {
			//如果航迹t门限内没有任何量测，则只进行更新
			track.numberOfConsecutiveInvisible++;
			double x = track.UKF.x(0, 0); //保存历史点迹
			double y = track.UKF.x(2, 0);
			track.track_point_seq.push(TrackPoint2D_t(x, y));
		}
		else {
			//1.计算L[t]
			std::set<size_t>& measure_set = A[t];
			for (auto it = measure_set.begin(); it != measure_set.end(); ++it) {
				size_t j = *it;
				setUnion(L[t], C.at(j), L[t]);
			}
			L[t].erase(t); //删除t本身

			//2.计算航迹t的归一化参数
			//计算与航迹t相关的量测的互联概率
			double normal_D_t = B;
			for (auto At_it = A[t].begin(); At_it != A[t].end(); At_it++) {
				size_t j = *At_it;
				double d_tj = D_tj(t, j, pred_z_dist, plots, A, L);
				D(t, j) = d_tj;
				normal_D_t += d_tj;
			}

			track.numberOfVisibleFrame++;
			track.numberOfConsecutiveInvisible = 0;
			track.timeSecFromEpoch = current_timeSecFromEpoch; //更新时间
			jpda_measure_t measureVec;
			measureVec.b_t0 = B / normal_D_t; //B_t0 ,航迹不与任何量测关联的概率
			for (auto it = A[t].begin(); it != A[t].end(); it++) {
				size_t j = *it;
				double B_tj = D(t, j) / normal_D_t;
				jpda_z_t zv;
				zv.likelihood = B_tj;
				zv.z = VectorXd::Zero(2);
				zv.z << plots[j].Range, plots[j].Azimuth;
				zv.plot_index = j;
				measureVec.measure.push_back(zv);
				CoastM(t, j) = B_tj;
			} //for
		}
	} //for
#define ASS_PROP_THRESHOLD	(0.0)
	if (algo_type == NNJPDA && trackSize > 0) {
		MatrixXd::Index maxRow, maxCol;

		stop_timer timer;
		int cc = 0;
		auto  start_time = timer.start();
		for (;;) {
			cc++;




			double maxValue = CoastM.maxCoeff(&maxRow, &maxCol);
			if (maxValue <= ASS_PROP_THRESHOLD)
				break;
			size_t trackid_min_st = maxRow;

			size_t optTrack = trackid_min_st;
			Vector2d z;
			z << plots[maxCol].Range, plots[maxCol].Azimuth;
			tracks[optTrack].UKF.update(z);		//update
			tracks[optTrack].calc_velocity();  //更新速度信息

			double x = tracks[optTrack].UKF.x(0, 0); //保存历史点迹
			double y = tracks[optTrack].UKF.x(2, 0);
			tracks[optTrack].track_point_seq.push(TrackPoint2D_t(x, y));
			tracks[optTrack].ClutterDense = plots[maxCol].ClutterDense; //更新杂波密度信息

			CoastM.row(optTrack).fill(0);
			CoastM.col(maxCol).fill(0);

		}

		timer.stop();


		spdlog::info("================== CoastM.maxCoeff = {0} , {1}", cc, timer.esc_ms());
	} //NNJPDA
#endif
	cleanTrack(); //清理过期航迹
}





static void dumpTrackInfo(TrackInfo& track) {
	unsigned int TID = track.trackID;
	double x = track.UKF.x(0, 0);
	double vx = track.UKF.x(1, 0);
	double y = track.UKF.x(2, 0);
	double vy = track.UKF.x(3, 0);
	double vel = sqrt(POW2(vx) + POW2(vy));
	double x_err = sqrt(track.UKF.P(0, 0));
	double vx_err = sqrt(track.UKF.P(1, 1));
	double y_err = sqrt(track.UKF.P(2, 2));
	double vy_err = sqrt(track.UKF.P(3, 3));

	printf("ID:%d,VEL=%.1f,X_ERR=%.1f,Y_ERR=%.1f,VX_ERR=%.1f,VY_ERR=%.1f\r\n",
		TID, vel, x_err, y_err, vx_err, vy_err);
}



void JPDADataAssociation::onDeleteTrack(TrackInfo& track) {
	if (deletetrackAct)
		deletetrackAct(track);
}
void JPDADataAssociation::cleanTrack() {
	auto iter = tracks.begin();
	while (iter != tracks.end()) {
		TrackInfo& track = *iter;
		if (isToDelete(track)) {
			if (track.ready_to_report)
				onDeleteTrack(track);
			iter = tracks.erase(iter);
		}
		else {
			++iter;
		}
	}
}


void JPDADataAssociation::setMaxTrackVelocity(double value) {
	max_trackVel_ms = value;
}
void JPDADataAssociation::setPlatformMove(bool isMove) {
	isMovePlat = isMove;
}

bool JPDADataAssociation::isToDelete(TrackInfo& track) {
	unsigned int TID = track.trackID;
	double x = track.UKF.x(0, 0);
	double vx = track.UKF.x(1, 0);
	double y = track.UKF.x(2, 0);
	double vy = track.UKF.x(3, 0);
	double velocity_value = sqrt(POW2(vx) + POW2(vy));
	double x_err = sqrt(track.UKF.P(0, 0));
	double y_err = sqrt(track.UKF.P(2, 2));
	double residual_z_norm = track.residual_z_norm_value_queue.mean();
	double	MAX_VEL_THRESHOLD = max_trackVel_ms;
	//double  MAX_XY_STD_ERR = 200; //这里进行修改，误差随距离进行变化(mtt::getMaxErr)
	static const double MAX_Track_Quality_Threshond = 0.3;	//航迹质量上限
	static const int  MAX_ConsecutiveInvisible_Threshold = 20; //连续多少个周期没有任何量测关联，则删除目标
	double range = sqrt(x * x + y * y); //距离
	double MAX_XY_STD_ERR = mtt::getMaxErr(range); //目标在当前距离下允许的最大误差

	//最近历史下，误差放大一些
	if (track.history <= 10) {
		MAX_XY_STD_ERR *= 1.5;
		MAX_VEL_THRESHOLD = max_trackVel_ms * 1.5;
	}

	if (track.numberOfConsecutiveInvisible > MAX_ConsecutiveInvisible_Threshold) {
		return true;
	}

	const double MAX_RESDIUAL_Threshold = 3.0;
	if (track.history > 5 && residual_z_norm >= MAX_RESDIUAL_Threshold) {
		return true;
	}

	//方差过大
	if ((x_err > MAX_XY_STD_ERR) || (y_err > MAX_XY_STD_ERR)) {
		return true;
	}


	//速度过大
	if (velocity_value > MAX_VEL_THRESHOLD) {
		return true;
	}

	/*
	计算航迹质量(已关联量测次数 / 扫描总数)
	如果航迹质量低于某一阈值则认为目标丢失,对于静止目标进行特殊处理
	*/
	if (track.history > 15) {
		if (track.history == 0)
			track.history = 1;
		double trackQuality = (double)track.numberOfVisibleFrame / (double)track.history;
		double trackQualityThreshond = MAX_Track_Quality_Threshond;

		if (track.history < 10)
			trackQualityThreshond = 0; //航迹初始阶段不考虑航迹质量

		if (track.movement_status.status == Stop && track.movement_status.stop_duration > 30)
			trackQualityThreshond = 0; //长时间静止目标不考虑航迹质量

		if (trackQuality < trackQualityThreshond) {
			return true;
		}
	}
	//PASS
	return false;
}

#if 0 
bool JPDADataAssociation::isToDelete(TrackInfo& track) {
	/*
	目标消批规则:
	(1)连续N个周期没有任何量测
	(2)目标的跟踪不确定性（状态协方差矩阵）到达一个阈值
	(3)目标的速度到达一个不可能的最大值
	*/

	//多少圈没有目标没有任何量测关联则消批
	static const int  MAX_ConsecutiveInvisible_Threshold = 20;
	//最大的残差上限
	static const double  MAX_RESDIUAL_Threshold = 3.0;
	//航迹质量上限
	static const double MAX_Track_Quality_Threshond = 0.3;
	//XY方向上的最大误差限
	double  MAX_XY_STD_ERR = 200;
	double	MAX_VEL_THRESHOLD = max_trackVel_ms;

	if (track.history <= 10) {
		//前几圈误差适当放大一些
		MAX_XY_STD_ERR = MAX_XY_STD_ERR * 1.5;
		MAX_VEL_THRESHOLD = max_trackVel_ms * 1.5;
	}

	unsigned int TID = track.trackID;
	double x = track.UKF.x(0, 0);
	double vx = track.UKF.x(1, 0);
	double y = track.UKF.x(2, 0);
	double vy = track.UKF.x(3, 0);
	double velocity_value = sqrt(POW2(vx) + POW2(vy));
	double x_err = sqrt(track.UKF.P(0, 0));
	double y_err = sqrt(track.UKF.P(2, 2));

	//获取平均残差估计,如果残差过大则认为目标在随机游走
	double residual_z_norm = track.residual_z_norm_value_queue.mean();
	if (track.history > 5 && residual_z_norm >= MAX_RESDIUAL_Threshold) {
		spdlog::debug("DEL TRACK-ID:{0} ,Reason:残差过大 , History:{1}", track.trackID, track.history);
		return true;
	}

	/*连续多少个周期没有任何量测关联，则删除目标*/
	if (track.numberOfConsecutiveInvisible > MAX_ConsecutiveInvisible_Threshold) {
		spdlog::debug("DEL TRACK-ID:{0},Reason:连续多少个周期没有任何量测关联", track.trackID);
		return true;
	}

	//由于方差过大删除目标
	double max_xy_stderr = 0;
	if (isMovePlat) {
		//移动平台由于船舶本身的晃动问题，会造成目标的方差比静止平台的目标大一些
		max_xy_stderr = MAX_XY_STD_ERR * 1.5;
	}
	else {
		max_xy_stderr = MAX_XY_STD_ERR;
	}
	if ((x_err >= max_xy_stderr) || (y_err >= max_xy_stderr)) {
		spdlog::debug("DEL TRACK-ID:{0},Reason:方差过大,{1},{2} ", track.trackID, x_err, y_err);
		return true;
	}

	//速度过大
	if ((track.history > 5) && velocity_value > (MAX_VEL_THRESHOLD * 1.1)) {
		spdlog::debug("DEL TRACK-ID:{0},Reason:速度过大", track.trackID);
		return true;
	}

	/*
	计算航迹质量(已关联量测次数 / 扫描总数)
	如果航迹质量低于某一阈值则认为目标丢失,对于静止目标进行特殊处理
	*/
	if (track.history > 15) {
		if (track.history == 0)
			track.history = 1;
		double trackQuality = (double)track.numberOfVisibleFrame / (double)track.history;
		double trackQualityThreshond = MAX_Track_Quality_Threshond;

		if (track.history < 10)
			trackQualityThreshond = 0; //航迹初始阶段不考虑航迹质量

		if (track.movement_status.status == Stop && track.movement_status.stop_duration > 30)
			trackQualityThreshond = 0; //长时间静止目标不考虑航迹质量

		if (trackQuality < trackQualityThreshond) {
			spdlog::debug("DEL TRACK-ID:{0},Reason:航迹质量差, Histoy:{1}", track.trackID, track.history);
			return true;
		}
	}
	//PASS
	return false;
}

#endif

double JPDADataAssociation::CostMinSolve(const MatrixXd& DistMatrix, std::vector<int>& Assignment) {


#if 1 
	double cost = HungAlgo.Solve(DistMatrix, Assignment);
	return  cost;
#else


	/*
	extern cost lap(int dim, cost **assigncost,
				col *rowsol, row *colsol, cost *u, cost *v);
	*/

	size_t rows = DistMatrix.rows();
	size_t cols = DistMatrix.cols();

	int dim = 0;
	double** costMatrix = NULL;
	if (rows < cols) {
		dim = cols;
		costMatrix = new double* [dim];
		for (int i = 0; i < dim; i++) {
			costMatrix[i] = new double[dim];
		}

		for (int i = 0; i < dim; ++i) {
			for (int j = 0; j < dim; ++j) {
				if (i >= rows)
					costMatrix[i][j] = 1000000;
				else
					costMatrix[i][j] = costMatrix[i][j];
			}
		}
	}

	if (rows > cols) {
		dim = rows;
		costMatrix = new double* [dim];
		for (int i = 0; i < dim; i++) {
			costMatrix[i] = new double[dim];
		}

		for (int i = 0; i < dim; ++i) {
			for (int j = 0; j < dim; ++j) {
				if (j >= cols)
					costMatrix[i][j] = 100000;
				else
					costMatrix[i][j] = costMatrix[i][j];
			}
		}
	}

	int* rowsol;         // An array to store column indexes assigned to row in solution  
	int* colsol;         // An array to store row indexes assigned to column in solution 
	double* u;             // u          - dual variables, row reduction numbers
	double* v;             // v          - dual variables, column reduction numbers
	rowsol = new int[dim];
	colsol = new int[dim];
	u = new double[dim];
	v = new double[dim];

	double totalCost = lap(dim, costMatrix, rowsol, colsol, u, v);  // Use lap algorithm to calculate the minimum total cost

	for (int i = 0; i < rows; i++)
		Assignment.push_back(rowsol[i]);

	return totalCost;
#endif
}

double JPDADataAssociation::getassociatePlotGate(double R, MovementStatusCode mvoeState) {
	if (mvoeState == Stop) {
		//静止目标
		double gate = mtt::getMaxErr(R) / 2; 
		if (gate <= 100)
			gate = 100;
		return gate; 
	
	}
	else {
		double gate = mtt::getMaxErr(R);
		if (gate <= 300)
			gate = 300;
		return gate;
	}
}